#ifndef FINDINGPATH_H
#define FINDINGPATH_H

#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <saint_nr_9/GoalArea.h>
#include <saint_nr_9/RobotPosition.h>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <cmath>
#include <sstream>
#include <queue>

using std::vector;
using std::string;
using std::priority_queue;
using std::stringstream;
using std::ofstream;
using std::cout;
using std::endl;

static const char NORMAL = 'n';
static const char SEEN = 's';
static const char ENTRANCE = 'e';
static const char CURIOUS = 'c';

#define X_DIRECTION 0
#define Y_DIRECTION 1
#define MAX_POINTS 20


struct vec2D
{
  int x;
  int y;
  
  vec2D(int x1, int y1)
  {
    x = x1;
    y = y1;
  }
  
  vec2D()
  {
  
  }
};

struct force_node
{
  int value;
  
  vec2D force;
  
  vec2D position;
};

class Compare_force_node {
public:
    bool operator()(force_node& f1, force_node& f2)
    {
       if (f1.value < f2.value) return true;
       else return false;
    }
};

struct map_element
{
  char element_type;
  vec2D force;
  
  map_element()
  {
    force.x = -2;
    force.y = -2;
    element_type = NORMAL;
  }
  
  std::string print()
  {
    stringstream s;
    if (force.x == -1)
    {
      s << "^";
    }
    else if (force.x == 1)
    {
      s << "v";
    }
    else if(force.y == -1)
    {
      s << "<";
    }
    else if (force.y == 1)
    {
      s << ">";
    }
    else
    {
      s << "w";
    }
    return s.str();
  }
};



void readMap(std::string path);
int getDirection(vec2D force);
bool same_direction(vec2D force, int start_direc);
void computeFollowers(int x, int y, int start_direc, int value, std::priority_queue<force_node, vector<force_node>, Compare_force_node> &pq);
void computeForces(int x_start, int y_start, int x_length, int y_length, int start_direc);
void set_goal_area(const saint_nr_9::GoalArea::ConstPtr& area);
void update_position(const saint_nr_9::RobotPosition::ConstPtr& position);
void initialize_map(const std_msgs::String::ConstPtr& signal);
bool equal_heading(vec2D force, int heading);
void reset_field_forces();
#endif
